package Program;

import java.util.ArrayList;
import java.util.List;

import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Move.MoveType;
import lejos.robotics.navigation.Pose;
import simulator.Tone;
import simulator.labyrinth.BarCode;
import simulator.labyrinth.Labyrinth;
import simulator.labyrinth.Square;
import simulator.labyrinth.Square.SquareState;
import simulator.labyrinth.SquareType;
import simulator.labyrinth.Wall;
import simulator.position.Point;
import simulator.position.Position;
import simulator.robot.BarCodeScanner;
import simulator.robot.Simulator;
import simulator.robot.SimulatorListener;
import Robot.RobotControl;
import Robot.RobotDirection;
import Robot.RobotListener;
import Robot.SensorColour;
import bluetooth.ProtocolPilot;

public class Execute extends Thread implements SpeedListener, LightValueListener, SonarAngleListener {
	
	private static final int SENSOR_RIGHT = 90;
	private static final int SENSOR_LEFT = -90;

	private static final int SECTOR_SIZE = 40;
	
	private ProtocolPilot pilot;
	private Simulator simulator;
	private RegulatedMotor simulatorSonarMotor;
	private BarCodeScanner barCodeScanner;

	private Listener listener;
	private int middleTime;
	private int lineTime;
	private Labyrinth laby = new Labyrinth();
	
	private RobotControl control;
	
	public Execute(RobotControl rc) throws Exception
	{
		this.control = rc;
		this.simulator = rc.getSimulator();
		SimulatorListener simListener = new SimulatorListener(simulator, control);
		this.listener = simListener;
		listener.start();
		simulatorSonarMotor = simulator.getSensorMotor();
		barCodeScanner = new BarCodeScanner(simulator, simListener);
		
		control.addSpeedListener(this);
		control.addLightValueListener(this);
	}
	
	private int whiteCalibration = -1;
	private int blackCalibration = -1;
	private int brownCalibration = -1;
	
	public synchronized void run() {
		while (true) {
			try {
				if(control.isColorCalibration())
				{
					control.setColorCalibration(false);
					SensorColour colorToCalibrate = control.getColorToCalibrate();
					if(colorToCalibrate != SensorColour.NONE && (pilot != null))
					{
						int result = pilot.calibrateColor(colorToCalibrate);
						if(colorToCalibrate == SensorColour.BLACK)
							blackCalibration = result;
						else if(colorToCalibrate == SensorColour.BROWN)
							brownCalibration = result;
						else if(colorToCalibrate == SensorColour.WHITE)
							whiteCalibration = result;
						
						if(whiteCalibration != -1 && brownCalibration != -1 && blackCalibration != -1)
						{
							control.BLACK_BROWN = (blackCalibration + brownCalibration) / 2;
							control.BROWN_WHITE = (brownCalibration + whiteCalibration) / 2;
							System.out.println("Calibration done:");
							System.out.println("Black - Brown = " + control.BLACK_BROWN);
							System.out.println("Brown - White = " + control.BROWN_WHITE);
						}
					}
				}
				
				if (control.isResetPosition()) {
					resetPosition();
				}
				if (control.isChangePilot())
					changePilot();
				if (control.isDiscover()) {
					discover();
				}
				if (control.isPlayTune())
				{
					//TODO
					if(pilot!=null){
					int code = pilot.scanBarcode();
					control.setPlayTune(false);}
					else{
						findAndOrientateOnLine(SensorColour.BLACK);
						((SimulatorListener) listener).scanBarcode();
					}
				}
				if (control.isRemoteControl()) {
					if (listener != null)
						listener.setWaitTime(500);

					double travelDistance = control.getTravelDistance();
					double rotateAngle = control.getRotateAngle();
					if (travelDistance != 0) {
						control.addAction("Travelling " + travelDistance + "cm.");
						travel(travelDistance);
						control.setTravelDistance(0);
					} else if (rotateAngle != 0) {
						control.addAction("Rotating " + rotateAngle + "�.");
						rotate(rotateAngle);
						control.setRotateAngle(0);
					} else if (control.isGoToMiddle()){
						control.addAction("Going to middle of current sector.");
						goToMiddle2();
						control.setGoToMiddle(false);
					} else {
						RobotDirection direction = control.getDriveDirection();
						control.setCurrentDirection(direction);
						switch (direction) {
							case FORWARD:
									forward();
									control.addAction("Going forward.");
								break;
							case BACKWARD:
									backward();
									control.addAction("Going backward.");
								break;
							case LEFT:
									rotateLeft();
									control.addAction("Turning left.");
								break;
							case RIGHT:
									rotateRight();
									control.addAction("Turning right.");
								break;
							case NONE:
									stopMovement();
									control.addAction("Stopped.");
								break;
						}
					}
				
					if (listener != null)
						listener.resetWaitTime();
				} else if (control.isWhiteLine()) {
					findAndOrientateOnLine(SensorColour.WHITE);
					control.setWhiteLine(false);
				} else if (control.drivePolygon()) {
					// Deze methode returned pas als hij klaar is.
					drivePolygon(control.getNumberOfSides(), control.getLengthOfSides());		
					
					control.setDrivePolygon(false);
				}
				
				this.wait();
			} catch (InterruptedException e) {
				control.addDebug(e.getMessage());
			}
		}
	}
	
	@Override
	public void handleTravelSpeedChange(double speed) {
		if(speed > getMaxTravelSpeed())
			control.setTravelSpeed(getMaxTravelSpeed());
		else
			setTravelSpeed(speed);
	}
	
	@Override
	public void handleRotateSpeedChange(double speed) {
		if(speed > getRotateMaxSpeed())
			control.setRotateSpeed(getRotateMaxSpeed());
		else
			setRotateSpeed(speed);
	}

	@Override
	public void handleSonarAngleChange(double angle) {
		if (Double.compare(getTachoCount(), angle) != 0 ) {
			rotateSonarMotorTo((int)angle);
		}
	}
	/**
	 * 
	 */
	private void resetPosition() {
		Pose newPose = new Pose(20, 20, 0);
		control.setRobotPose(newPose);
		
		if (pilot != null) {
			pilot.setPose(newPose);
		}
		
		simulator.reset();
		control.setResetPosition(false);
	}
	
	public SimulatorListener getSimulatorListener() {
		return (SimulatorListener) listener;
	}
	
	private void drivePolygon(int numberOfSides, int lengthOfSides) {
		control.addAction("Starting polygon.");
		
		double angleToRotate = 360 / (double) numberOfSides;
		RobotDirection rotate;
		if(!control.getPolygonTurnDirection())
			angleToRotate *= -1;
		
		if (angleToRotate < 0)
			rotate = RobotDirection.LEFT;
		else
			rotate = RobotDirection.RIGHT;
		
		for (int i = 0; i < numberOfSides; i++) {
			control.setCurrentDirection(RobotDirection.FORWARD);
			control.addAction("Driving " + lengthOfSides + " cm.");
			travel(lengthOfSides);
			control.addAction("Rotating " + angleToRotate + "�.");
			control.setCurrentDirection(rotate);
			rotate(angleToRotate);
		}
		control.setCurrentDirection(RobotDirection.NONE);
		stopMovement();
		control.addAction("Completed.");
	}

	/**
	 * 
	 * @pre		De robot moet een orientatie loodrecht op een witte lijn hebben.
	 * 
	 * @return	De robot staat in het midden van een vakje. Dit kan ��n van de
	 * 			vakjes zijn volgens de richting van de robot als hij op een 
	 * 			kruising staat.
	 * 
	 * @throws InterruptedException
	 */
	private void goToMiddle() throws InterruptedException {
		listener.setWaitTime(20);
		// go to the middle of the sector
		float sonarOffset = 2;
		float sonarDistance;
		Thread.sleep(20);
		sonarDistance = control.getSonarDistance();
		travel( (sonarDistance % SECTOR_SIZE) - (SECTOR_SIZE / 2.0), sonarOffset, false );
		listener.resetWaitTime();
		
		rotate(90);
		Thread.sleep(20);
		sonarDistance = control.getSonarDistance();
		travel((sonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, sonarOffset, false);
		rotate(-90);
	}
	private synchronized void findMiddle() throws InterruptedException{
		
		double prevOrientation = getPosition().getOrientation();
		
		if(lineTime >= 3)
			middleTime = 3;
		if(middleTime <= 2 || laby.getSquareFromPoint(this.getPosition().getPoint())==null)
			middleTime++;
		else{
			if(lineTime >= 3 && laby.getSquareFromPoint(this.getPosition().getPoint())!= null &&laby.getSquareFromPoint(this.getPosition().getPoint()).getBarCodes().size()==0){
				lineTime = 0;
				control.addAction("Orientate on line to fix orientation.");
				findAndOrientateOnLine(SensorColour.WHITE);
				
			}
			SquareType type = laby.getSquareFromPoint(this.getPosition().getPoint()).getType();
			
			switch(type){
			case CROSSING:
				middleTime++;
				break;
			
			case CORNER:
				goToMiddle2();
				middleTime = 0;
				break;
			case STRAIGHT:
				setStraight();
				break;
			case T_CROSSING:
				setStraight();
				break;
			case DEAD_END:
				goToMiddle2();
				middleTime = 0;
				break;
			case NONE:
				assert false;
				break;
			}
		}
		
		executeMovement(new Move(MoveType.ROTATE, 0, (float) (prevOrientation - getPosition().getOrientation()), false));
		
		lineTime++;
		System.out.println(lineTime + "//"+ middleTime);
		Thread.sleep(400);
	}
	private synchronized void setStraight() throws InterruptedException {
		
		control.addAction("Fixing position in one direction.");
		
		if(pilot != null){
			pilot.setStraight();
			return;}
		float xSonarOffset = 4;
		float ySonarOffset = 1;
		rotateSonarMotorTo(0);
		Thread.sleep(200);
		if(control.getSonarDistance()<SECTOR_SIZE){
			travel((control.getSonarDistance() % SECTOR_SIZE) - SECTOR_SIZE / 2, xSonarOffset, false);
		}
		else{
			rotateSonarMotorTo(90);
			Thread.sleep(200);
			if(control.getSonarDistance()<SECTOR_SIZE){
				double distance = control.getSonarDistance();
				rotate(90);
				travel((distance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
				rotate(-90);
			}
			else{
				rotateSonarMotorTo(-90);
				Thread.sleep(200);
				double distance = control.getSonarDistance();
				rotate(-90);
				travel((distance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
				rotate(90);
			}
		}
		rotateSonarMotorTo(0);
		Thread.sleep(200);
	}

	private synchronized void goToMiddle2() throws InterruptedException {
		middleTime = 0;
		
		control.addAction("Going to middle of current square.");
		
		if(pilot != null){
			pilot.goToMiddle();
			return;
		}
		listener.setWaitTime(20);
		// go to the middle of the sector
		float xSonarOffset = 0;
		float ySonarOffset = 0;
		float xSonarDistance;
		float ySonarDistance;
		rotateSonarMotorTo(0);
		Thread.sleep(500);
		xSonarDistance = control.getSonarDistance();
		rotateSonarMotorTo(90);
		Thread.sleep(500);
		ySonarDistance = getSafeDistance();
		if(xSonarDistance>40){
			if(ySonarDistance>40){
				rotateSonarMotorTo(-90);
				Thread.sleep(500);
				if(getSafeDistance()<ySonarDistance){
					ySonarDistance = getSafeDistance();
					rotate(-90);
					travel((ySonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
					if(getSafeDistance()<xSonarDistance){
						xSonarDistance = getSafeDistance();
						rotate(-90);
						travel((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
						rotate(180);
					}
					else{
						rotate(90);
						travel((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, xSonarOffset, false);
						}
					}
				else{
					rotate(90);
					rotateSonarMotorTo(90);
					Thread.sleep(500);
					travel((ySonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset,false);
					if(getSafeDistance()<xSonarDistance){
						xSonarDistance = control.getSonarDistance();
						rotate(90);
						travel((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
						rotate(180);
						}
					else{
						rotate(-90);
						travel((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, xSonarOffset, false);
						}
					}
				
				}
				else{
					rotate(90);
					travel((ySonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
//					System.out.println(ySonarDistance);
					Thread.sleep(500);
					if(control.getSonarDistance()<xSonarDistance){
						xSonarDistance = control.getSonarDistance();
						rotate(90);
						travel((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
						rotate(180);
					}
					else{
						rotate(-90);
						travel((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, xSonarOffset, false);
					}
				}
			}
		else{
			travel((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, xSonarOffset, false);
			if(ySonarDistance>40){
				rotateSonarMotorTo(-90);
				Thread.sleep(200);
				if(getSafeDistance()<ySonarDistance){
					ySonarDistance = getSafeDistance();
					rotate(-90);
					travel((ySonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
					rotate(90);
				}
				else{
					rotate(90);
					travel((ySonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
					rotate(-90);
					}
			}
			else{
					rotate(90);
					travel((ySonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2, ySonarOffset, false);
					rotate(-90);
			}
			
		}
		System.out.println("ydistance: " + (int) ((ySonarDistance % SECTOR_SIZE) - (SECTOR_SIZE / 2))+ ", xdistance="+ ((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2));
		rotateSonarMotorTo(0);
	}
			

	/**
	 * @throws InterruptedException 
	 * 
	 */
	private double findValidWay() throws InterruptedException {
		if (listener != null)
			listener.setWaitTime(20); 

		rotateSonarMotorTo(0);
		Thread.sleep(50);
		
		float sonarDistance;
		sonarDistance = getSafeDistance();
		
		if (sonarDistance < SECTOR_SIZE) {		// forward is a wall
			rotateSonarMotorTo(SENSOR_RIGHT);
		} else
			return 0.0;
		
		Thread.sleep(50);
		sonarDistance = getSafeDistance();
		
		if (sonarDistance < SECTOR_SIZE) {		// right is a wall
			rotateSonarMotorTo(SENSOR_LEFT);
		} else {
			rotateSonarMotorTo(0);
			return 90.0;
		}
		
		Thread.sleep(50);
		sonarDistance = getSafeDistance();
		
		if (sonarDistance < SECTOR_SIZE) {		// left is a wall
			rotateSonarMotorTo(0);
			return 180.0;
		} else {
			rotateSonarMotorTo(0);
			return -90.0;
		}
	}
	
	/**
	 * @param	scans	The values of a scans in following directions:
	 * 					front, left, right. -1 if the scan is greater than the
	 * 					size of the sector.
	 * @param 	pos		The current position of the robot.
	 * 
	 * @pre		De robot moet een orientatie loodrecht op een witte lijn hebben.
	 * 
	 * @return 	0 if scans is null or the length is not three or pos is null.
	 * 			the angle to turn to the adjacent square that is yet not discovered.
	 * 			Double.NaN if all adjacent squares are discovered.
	 * @throws InterruptedException 
	 */
	private double findValidWay(double[] scans, Position pos) throws InterruptedException {
		if (scans == null || scans.length != 3 || pos == null)
			return 0;

		Square next = laby.getAdjacentSquareGoing(pos, 0);
		if (scans[0] == -1 && 
				(next == null || next.getState() != SquareState.DISCOVERED)	)	// forward is no wall
			return 0.0;		
		
		next = laby.getAdjacentSquareGoing(pos, -90.0); //nextPos
		if (scans[1] == -1 && 
				(next == null || next.getState() != SquareState.DISCOVERED)	)	// left is no wall
			return -90.0;

		next = laby.getAdjacentSquareGoing(pos, 90.0);
		if (scans[2] == -1 && 
				(next == null || next.getState() != SquareState.DISCOVERED)	)	// right is no wall
			return 90.0;
			
		next = laby.getAdjacentSquareGoing(pos, 180.0);
		if (next == null || next.getState() != SquareState.DISCOVERED)
			return 180.0;
		
		return Double.NaN;
	}
	
	private void discover() {
		laby = new Labyrinth();
		Program.simulatorGui.setLabyrinth(laby);
		
		try {
			findAndOrientateOnLine(SensorColour.WHITE);
			goToMiddle2();
			if(pilot!=null)
				resetPosition();
			
			if(listener instanceof RobotListener)
				((RobotListener) listener).waitForRefresh();
			float sonarDistance = getSafeDistance();
			
			if (sonarDistance < SECTOR_SIZE) {
				int orientation = (int)(getPosition().getOrientation() % 360);
				laby.addWall(calculateWall(getPosition(), orientation, SECTOR_SIZE));
			}
			
			rotate(180.0);
			Thread.sleep(50);
			
			int travelResult = -1;
			Square currentSquare;
			
			while (control.isDiscover()) {
			
				currentSquare = laby.getSquareFromPoint(getPosition().getPoint());				
				if(currentSquare == null)
				{
					currentSquare = new Square();
					laby.addSquare(Labyrinth.getStartPointOfSquare(getPosition().getPoint()), currentSquare);
				}
				
				double[] scans;
				
				if (travelResult == 1) {
					// tijdens het rijden zag de robot zwart,
					if (currentSquare.getBarCodes().isEmpty()) {
						// het vakje heeft nog geen barcode dus scan de barcode!
						control.addAction("Black seen, scanning barcode.");
						int number = scanBarcode();
						
						if (number != -1) {
							Position posBarcode = new Position(getPosition().getX(), getPosition().getY(), (getPosition().getOrientation() + 90) % 180);
							BarCode barcodePresent = new BarCode(number, posBarcode);
							
							laby.addBarcode(getPosition().getPoint(), barcodePresent);
							control.addAction("Performing action: "+barcodePresent.getAction().toString());
							performAction(barcodePresent);
							
//							if (barcodePresent.getAction() != Action.FINISH)	//TODO wegdoen (alleen om te teste)
//								currentSquare.setAsCheckPoint();
						}
						findMiddle();
						currentSquare.setState(SquareState.DISCOVERED);
					}
					// barcodes kunnen alleen in een straat voorkomen, dus voeg de muren toe.
					double[] barcodeScans = {-1, SECTOR_SIZE / 2, SECTOR_SIZE / 2};
					scans = scanSector(barcodeScans);
					
				} else if (travelResult > 0) {
					if (travelResult == 2) {
						if (control.isTouchLeft()) {
							if (control.isTouchRight())
								laby.addWall(calculateWall(getPosition(), getPosition().getOrientation(), SECTOR_SIZE));
							else
								laby.addWall(calculateWall(getPosition(), getPosition().getOrientation() - 90, SECTOR_SIZE));
						} else
							laby.addWall(calculateWall(getPosition(), getPosition().getOrientation() + 90, SECTOR_SIZE));
						
						travel(-10);
						findAndOrientateOnLine(SensorColour.WHITE);
					} else
						laby.addWall(calculateWall(getPosition(), getPosition().getOrientation(), SECTOR_SIZE));
					// de robot is gebotst of 10 cm van een muur gestopt dus naar het midden van een vakje gaan.
					goToMiddle2();
					setPosition(calibratePosition(getPosition()));
					
					scans = scanSector(null);
					currentSquare.setState(SquareState.DISCOVERED);
					scans = scan();
				} else {
					scans = scanSector(null);
					currentSquare.setState(SquareState.DISCOVERED);
					findMiddle();
					scans = scan();
				}
				
				double angle = findValidWay(scans, getPosition());
				Program.simulatorGui.repaint();

				
				travelResult = -1;
				if (Double.isNaN(angle)) {
					
					control.addAction("All adjacent squares are discovered. Searching closest undiscovered square.");
					List<Move> toPerform = laby.getPathToClosestSquareSatisfying(getPosition(), UndiscoverdSquare.getInstance(), true);
					//	Geen onontdekt vakje meer, ga naar de finish
					if (toPerform == null) {
						control.addAction("No undiscovered square found. Going to finish.");
						try {
							toPerform = laby.getShortestPathToFinish(getPosition());
							
							//zet eerst goed.
							goToMiddle2();
							
							for(Move move: toPerform)
								executeMovement(move);
								
							
						} catch (IllegalStateException e) {
							control.addDebug(e.getMessage());
						} finally {	
							control.setDiscover(false);
							control.addAction("Finished discovering labyrinth");
						}
					} else {
						control.addAction("Going to square found.");
						for(Move move: toPerform){
							travelResult = executeMovement(move);
							if (move.getMoveType().equals(MoveType.TRAVEL)) {
								middleTime++;
								lineTime++;
							} 

						}
					}
				} else {
					rotate(angle);
					if (currentSquare.getBarCodes().isEmpty())
						travelResult = travelSafely(SECTOR_SIZE);
					else {
						travel(SECTOR_SIZE / 2);
						travelResult = travelSafely(SECTOR_SIZE / 2);
					}
				}
			}
			
		} catch (InterruptedException e) {
			control.setDiscover(false);
		}

//		Program.simulatorGui.setLabyrinth(null);	//TODO
	}
	
	private int executeMovement(Move toExecute) {
		switch(toExecute.getMoveType()) {
			case TRAVEL:
				if (toExecute.isMoving())
					return travelSafely(toExecute.getDistanceTraveled());
				else
					travel(toExecute.getDistanceTraveled());
				break;
			case ROTATE:
				double toTurn = toExecute.getAngleTurned();
				double angle = MathExtra.getDiscreteAngle(toTurn);
				//	-90		->		270
				// -270		->		90	
				
				if (angle == 90 && toTurn < 0)
					toTurn += 360;
				else if (angle == 270 && toTurn > 0)
					toTurn -= 360;
				
				rotate(toTurn);
				break;
			default:
				break;
		}
		
		return -1;
	}
	
	private synchronized double[] scan(){
		double[] result  = {-1, -1, -1};
		for(Wall wall: laby.getSquareFromPoint(getPosition().getPoint()).getWalls()){
		result[0]=Math.max( getPosition().getDistanceTo(wall),result[0]);
		result[1]=Math.max( new Position(getPosition().getX(),getPosition().getY(),getPosition().getOrientation()+270).getDistanceTo(wall),result[1]);
		result[2]=Math.max( new Position(getPosition().getX(),getPosition().getY(),getPosition().getOrientation()+90).getDistanceTo(wall),result[2]);
		}
		return result;
	}
	/**
	 * @pre		De robot moet een orientatie loodrecht op een witte lijn hebben.
	 * @pre		Scans moet null of een array van lengte 3 zijn.
	 * 
	 * @param	scans
	 * 			Een array met scanwaarden als je aan de hand van deze waarden de
	 * 			muren wil toevoegen aan de doolhof.
	 * 
	 * @effect	Voegt muren aan de doolhof toe op de plaatsen waar de scan
	 * 			kleiner is dan de grootte van een vakje.
	 * 
	 * @return	An array with distances to wall or -1 if wall is not in sector.
	 * 			First entry is front, then left, right.
	 * 			Null if the labyrinth is null.
	 * 
	 * @throws InterruptedException
	 */
	private synchronized double[] scanSector(double[] scans) throws InterruptedException {

		if (laby == null)
			return null;
			
		
		double[] result  = {-1, -1, -1};
		//control.setSensorAngle(0);
		Position pos = getPosition();
		int orientation = (int)(pos.getOrientation() % 360);
		Square toScan = laby.getSquareFromPoint(pos.getPoint());
		
		if(toScan == null) {
			toScan = new Square();
			laby.addSquare(Labyrinth.getStartPointOfSquare(pos.getPoint()), toScan);
		}
		
		Wall front = calculateWall(pos, orientation, SECTOR_SIZE);
		Wall left = calculateWall(pos, (orientation - 90) % 360, SECTOR_SIZE);
		Wall right = calculateWall(pos, (orientation + 90) % 360, SECTOR_SIZE);
		
		if (scans == null) {
			if (listener != null)
				listener.setWaitTime(20); 
			
			float sonarDistance;
			List<Wall> walls = toScan.getWalls();
			
			if (! walls.contains(front)) { // in front is not yet found a wall
				sonarDistance = getSafeDistance();
				if (sonarDistance < SECTOR_SIZE) // in front is a wall
					result[0] = sonarDistance;
			}
			
			if (! walls.contains(left)) {
				rotateSonarMotorTo(SENSOR_LEFT);
				Thread.sleep(30);
				sonarDistance = getSafeDistance();
				
				if (sonarDistance < SECTOR_SIZE)	// left is a wall
					result[1] = sonarDistance;
			}
			
			if (! walls.contains(right)) {
				rotateSonarMotorTo(SENSOR_RIGHT);
				Thread.sleep(30);
				sonarDistance = getSafeDistance();
				
				if (sonarDistance < SECTOR_SIZE)	// right is a wall
					result[2] = sonarDistance;
			}
			
		} else
			result = scans.clone();
		
		if (result[0] > 0 && result[0] < SECTOR_SIZE) {// in front is a wall
			laby.addWall(front);
			Program.simulatorGui.repaint();
		}
		
		if (result[1] > 0 && result[1] < SECTOR_SIZE) {	// left is a wall
			laby.addWall(left);
			Program.simulatorGui.repaint();
		}
		
		
		if (result[2] > 0 && result[2] < SECTOR_SIZE) {	// right is a wall
			laby.addWall(right);
			Program.simulatorGui.repaint();
		}
		
		rotateSonarMotorTo(0);
		return result;
	}
	
	private Wall calculateWall(Position pos, double or, double sizeSector) {
		Point sectorStart = Labyrinth.getStartPointOfSquare(pos.getPoint());
		
		double beginSectorX = sectorStart.getX();
		double beginSectorY = sectorStart.getY();
		/*
		if(pos.getX() < 0)
			beginSectorX = (int)((pos.getX() / sizeSector) * sizeSector) - sizeSector; 
		else
			beginSectorX = (int)(pos.getX() / sizeSector) * sizeSector;
		
		if(pos.getY() < 0)
			beginSectorY = (int)((pos.getY() / sizeSector) * sizeSector) - sizeSector; 
		else
			beginSectorY = (int)(pos.getY() / sizeSector) * sizeSector;
		*/
		
		
		int discOr = MathExtra.getDiscreteAngle(or);
		
		double botX, botY, topX, topY;
        botX = beginSectorX;
        botY = beginSectorY;
        topX = botX + sizeSector;
        topY = botY + sizeSector;
        
		
		switch(discOr) {
			case 0:
				return new Wall(new Point(topX, botY), new Point(topX, topY));
			case 90:
				return new Wall(new Point(botX, topY), new Point(topX, topY));
			case 180:
				return new Wall(new Point(botX, botY), new Point(botX, topY));
			case 270:
				return new Wall(new Point(botX, botY), new Point(topX, botY));
			default:
				return null;
		}
	}

	/**
	 * 
	 */
	private void findAndOrientateOnLine(SensorColour colour) {
		
		
		control.addAction("Seeking white line.");
		try {
			if(pilot != null) {
				pilot.findWhiteLine();
				control.addAction("Succes.");
			}
			else
			{	
				findLine(colour);
				stopMovement();		
				control.addAction("Found white line.");
				control.addAction("Orientating");
				orientateOnLine(colour);
				control.addAction("Succes.");
				travel(-SECTOR_SIZE/2+3);
			}
		} catch (InterruptedException e) {
			control.addDebug(e.getMessage());
		}
	}
	
	private boolean findLine(SensorColour colour) throws InterruptedException {
		if (listener != null)
			listener.setWaitTime(10);
		
//		float sonarDistance = control.getSonarDistance();
//		rotateSonarMotorTo(SONAR_RIGHT, false);
//		Thread.sleep(50);
//		float sonar90 = control.getSonarDistance();
//		rotateSonarMotorTo(SONAR_LEFT, false);
//		Thread.sleep(50);
//		float sonarMin90 = control.getSonarDistance();
		
//		orientateToOpening(sonarDistance);
		
		// turn to be parallell with the wall
//		float toTravel = 5;
//		if (sonarDistance < 12)
//			toTravel = -5;
//		rotateSonarMotorTo(SONAR_LEFT, false);
//		Thread.sleep(50);
//		sonarDistance = control.getSonarDistance();
//		travel(toTravel);
//		Thread.sleep(50);
//		double sonarDistance1 = control.getSonarDistance();
//		
//		double angle = Math.atan2(Math.abs(sonarDistance1 - sonarDistance), 5);
//		angle = Math.toDegrees(angle);
//		System.out.println(angle);
//		if (sonarDistance > sonarDistance1)
//			rotate(180 - angle);
//		else
//			rotate(-180 + angle);
//		
//		stopMovement();
//		Thread.sleep(50);
//		
//		goToMiddle();
		
		// search for white line
		
		setTravelSpeed(5);
		if (listener != null)
			listener.setWaitTime(10);
		
		int tries = 0;
		double angle = findValidWay();
		if ((int)angle == 180)
			tries++;
		
		rotate(angle);
		if (control.getLightColour() != colour)
			forward();
		
		while(control.getLightColour() != colour) {
			if (control.getSonarDistance() < SECTOR_SIZE / 2) {
				angle = findValidWay();
				if ((int)angle == 180)
					tries++;
				else
					tries = 0;
				
				if (tries != 2)
					rotate(angle);
				else {
					if (!orientateToOpening()) {
						stopMovement();
						return false;
					}
					tries = 0;
				}
					
					
				forward();
			}
			Thread.sleep(1);
		}
		
		stopMovement();
		setTravelSpeed(control.getTravelSpeed());
		listener.resetWaitTime();
		
		return true;
	}

	/**
	 * @param sonarDistance
	 * @throws InterruptedException
	 */
	private boolean orientateToOpening() throws InterruptedException {
		if (listener != null)
			listener.setWaitTime(10); 
		
		float sonarDistance = control.getSonarDistance();
		
		if (sonarDistance > SECTOR_SIZE) {
			rotateLeft();
			while (sonarDistance > SECTOR_SIZE) {
				Thread.sleep(5);
				sonarDistance = control.getSonarDistance();
			}
			stopMovement();
		} else {
			rotateRight();
			while (sonarDistance < SECTOR_SIZE) {
				if (getMovement().getAngleTurned() >= 360) {
					control.addDebug("No opening found." +
							" Please check if I am not standing in a closed square.");
					return false;
				}
				Thread.sleep(5);
				sonarDistance = control.getSonarDistance();
			}
			stopMovement();
		}
		
		rotateRight();
		Thread.sleep(10);
		sonarDistance = control.getSonarDistance();
		while(sonarDistance > SECTOR_SIZE) {
			Thread.sleep(5);
			sonarDistance = control.getSonarDistance();
		}
		stopMovement();
		
		rotate(- getMovement().getAngleTurned() / 2);
		
		return true;
	}
	
	/**
	 * 
	 * @pre		De robot moet in het midden staan met een orientatie loodrecht
	 * 			op een witte lijn.
	 * @param 	pos
	 * 			De huidige positie van de robot.
	 * 
	 * @return	De gecalibreerde positie van de robot. De orientatie is een
	 * 			veelvoud van 90. De x- en y-coordinaten zijn een veelvoud van
	 * 			40 plus 20.
	 */
	private Position calibratePosition(Position pos) {
		return new Position(
				(int)(pos.getX() / SECTOR_SIZE) * SECTOR_SIZE + Math.signum(pos.getX()) * 20,
				(int)(pos.getY() / SECTOR_SIZE) * SECTOR_SIZE + Math.signum(pos.getY()) * 20,
				MathExtra.getDiscreteAngle(pos.getOrientation()));
	}

	private void changePilot() {
		boolean succes = true;
		if (pilot == null) {
			try {
				listener.stop();
			} catch (SecurityException e) {
				succes = false;
				control.addDebug(e.getMessage());
			}
			pilot = new ProtocolPilot();
			pilot.setTravelSpeed(control.getTravelSpeed());
			pilot.setRotateSpeed(control.getRotateSpeed());
			
			simulator.reset();
			listener = new RobotListener(pilot, control);
			listener.start();
			
		} else {
			try {
				listener.stop();
			} catch (SecurityException e) {
				succes = false;
				control.addDebug(e.getMessage());
			}
			pilot = null;
//			simulator = new Simulator();
			simulator.reset();
			control.setRobotPose(null);
			
			SimulatorListener simListener = new SimulatorListener(simulator, control);
			barCodeScanner = new BarCodeScanner(simulator, simListener);
			this.listener = simListener;
			listener.start();
		}
		
		control.setChangePilot(false);
		if (succes)
			control.changedPilot();
	}
	
	
	
	private void rotateLeft()
	{
		simulator.rotate(-1000, true);
		
		if (pilot != null)
			pilot.rotate(-1000, true);
	}
	
	private void rotateRight()
	{
		simulator.rotate(1000, true);
		
		if (pilot != null)
			pilot.rotate(1000, true);
	}
	
	private void setTravelSpeed(double travelSpeed) {
		simulator.setTravelSpeed(travelSpeed);
		
		if (pilot != null)
			pilot.setTravelSpeed(travelSpeed);
	}
	
	private void setRotateSpeed(double rotateSpeed) {
		simulator.setRotateSpeed(rotateSpeed);
		
		if (pilot != null)
			pilot.setRotateSpeed(rotateSpeed);
	}
	
	private void setPosition(Position pos) {
		simulator.setPosition(pos.getX(), pos.getY(), pos.getOrientation());
		
		if (pilot != null)
			pilot.setPose(new Pose((float) pos.getX(), 
									(float) pos.getY(),
									(float) pos.getOrientation()));
	}
	
	private void forward() {
		simulator.forward();

		if (pilot != null)
			pilot.forward();
	}
	
	private void backward() {
		simulator.backward();
		
		if (pilot != null)
			pilot.backward();
	}
	

	private void travel(double distance, double robotOffset, boolean immediateReturn)
	{
		if (pilot != null) {
			simulator.travel(distance, true);
			pilot.travel(distance + robotOffset, immediateReturn);
		} else
			simulator.travel(distance, immediateReturn);
	}
	
	private void travel(double distance)
	{
		travel(distance, 0, false);	// false ipv true
	}
	
	/**
	 * 
	 * @param distance
	 * 			de afstand om te rijden.
	 * @return	Een getal dat het resultaat van het rijden teruggeeft.
	 * 			Als het getal anders is dan 0 is de robot gestopt.
	 * 			0: geen probleem
	 * 			1: zwart gevonden
	 * 			2: gebotst
	 * 			3: sonarafstand kleiner dan 10
	 */
	private int travelSafely(double distance) {
		
		int result;
		
		if (pilot != null) {
			simulator.travel(distance, true);
			result = pilot.travelSafely(distance);
			if (result != 0)
				simulator.stop();
		}
		else {
			simulator.travel(distance, true);
			
			result = 0;
			while(simulator.isMoving()) {
				if ( control.getLightColour().equals(SensorColour.BLACK) ) {
					result = 1;
					simulator.stop();
					break;
				}
				if ( control.isTouchLeft() || control.isTouchRight() ) {
					result = 2;
					simulator.stop();
					break;
				}
				if ( control.getSonarDistance() < SECTOR_SIZE / 4) {
					result = 3;
					simulator.stop();
					break;
				}
				Thread.yield();
			}
		}

		return result;
	}
	
	
	private void rotate(double angle, double robotOffset) {
		if (pilot != null) {
			simulator.rotate(angle, true);
			pilot.rotate(angle + robotOffset, false);
		} else 
			simulator.rotate(angle);
	}
	
	private void rotate(double angle) {
		rotate(angle, 0);
	}
	
	
	private void rotateSonarMotorTo(int angle) {
		if (pilot != null) {
			simulatorSonarMotor.rotateTo(angle, true);	//TODO deze direct late returne en pilot niet.
			pilot.rotateSonar(angle);
		} else
			simulatorSonarMotor.rotateTo(angle, false);

	}
	
	private double getTachoCount() {
		if (pilot != null)
			return pilot.getSonarTachoCount();
		
		return simulatorSonarMotor.getTachoCount();
	}
	
	private synchronized void stopMovement() {
		simulator.stop();
		
		if (pilot != null) {
			pilot.stop();
		}
	}
	
	private double getMaxTravelSpeed() {
		if (pilot != null)
			return pilot.getMaxTravelSpeed();
		
		return simulator.getMaxTravelSpeed();
	}
	
	private double getRotateMaxSpeed() {
		if (pilot != null)
			return pilot.getMaxRotateSpeed();
		
		return simulator.getRotateMaxSpeed();
	}
	
	private Move getMovement() {
		if (pilot != null)
		{
			System.out.println("Getting movement");
			return pilot.getMovement();
		}
		
		return simulator.getMovement();
	}
	
	private Position getPosition() {
		if (pilot != null) {
			return new Position(pilot.getPose());
		}
		
		return simulator.getPosition();
	}
	
	/**
	 * @pre		De robot moet op of net voor een barcode staan.
	 * @return
	 */
	private synchronized int scanBarcode() {
		if (pilot != null)
			return pilot.scanBarcode();
		
		double travelSpeed = control.getTravelSpeed();
		setTravelSpeed(5);
		
		if (control.getLightColour() != SensorColour.BROWN) {
			 listener.setWaitTime(2);
			ArrayList<SensorColour> list = new ArrayList<SensorColour>();
			backward();
			boolean todo = true;
			while(todo) {
				list.add(control.getLightColour());
				try {
				Thread.sleep(10);
				} catch (InterruptedException e) {
					
				}
				if (list.size()>3&&list.get(list.size()-1)== SensorColour.BROWN&&list.get(list.size()-2)==SensorColour.BROWN&&list.get(list.size()-3)==SensorColour.BROWN) {
					stopMovement();
					todo = false;
				}
			}
		}
		listener.resetWaitTime();
		setTravelSpeed(travelSpeed);
		return barCodeScanner.scanBarcode().getNumber();
	}
	
	private void performAction(BarCode barCode) {
		switch (barCode.getAction()) {
			case TURN_LEFT:
				rotate(360);
				break;
			case TURN_RIGHT:
				rotate(-360);
				break;
			case PLAY_TUNE:
				//TODO
				try {
					Tone.HallOfMountainKing();
				} catch (InterruptedException e) {
					
					e.printStackTrace();
				}
				break;
			case WAIT:
				try {
					Thread.sleep(5000);
				} catch (InterruptedException e) {
					
					e.printStackTrace();
				}
				break;
			case DRIVE_SLOW:
				setTravelSpeed(control.getTravelSpeed()-10);
				break;
			case DRIVE_FAST:
				setTravelSpeed(control.getTravelSpeed()+10);
				break;
			case FINISH:
				laby.getSquareFromPoint(this.getPosition().getPoint()).setAsFinish();
				break;
				
			case CHECKPOINT:
				laby.getSquareFromPoint(this.getPosition().getPoint()).setAsCheckPoint();
				break;
				
			default:
				break;
		}
		
	}
	private float getSafeDistance(){
		if(pilot!=null)
			return pilot.getSafeSonarDistance();
		else
			if(((SimulatorListener)listener).getSonarDistance()<20||((SimulatorListener)listener).getSonarDistance()>40)
				return (float) ((SimulatorListener)listener).getSonarDistance();
			else
				return (float) ((SimulatorListener)listener).getSafeSonarDistance();
		}
	private void orientateOnLine(SensorColour colour)
	{	
		int sensorToWheelDist = simulator.getWheelToSensorDistance();
		listener.setWaitTime(10);
		double travelSpeed = control.getTravelSpeed();
		double rotateSpeed = control.getRotateSpeed();
		setTravelSpeed(5);
		setRotateSpeed(20);

		travel(sensorToWheelDist);
		rotateLeft();
		
		while(control.getLightColour() == colour) 
		{
			// TODO als binnen de 180 grade geen wit gevonden is, draai terug en kijk naar andere kant
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		
		while(control.getLightColour() != colour) 
		{
			// TODO als binnen de 180 grade geen wit gevonden is, draai terug en kijk naar andere kant
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		stopMovement();
		
		setRotateSpeed(5);
		rotateRight();
		while(control.getLightColour() == colour) 
		{
			// TODO als binnen de 180 grade geen wit gevonden is, draai terug en kijk naar andere kant
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		stopMovement();
		
		setRotateSpeed(70);
		setTravelSpeed(15);
		rotate(59.6,20.4);
		listener.resetWaitTime();
		setTravelSpeed(travelSpeed);
		setRotateSpeed(rotateSpeed);
		travel(-sensorToWheelDist);
	}
	

}

class UndiscoverdSquare implements Condition<Square> {

	private static final UndiscoverdSquare instance = new UndiscoverdSquare();
	
	public static UndiscoverdSquare getInstance() {
		return instance;
	}
	
	private UndiscoverdSquare() {
			
	}
	
	@Override
	public boolean satisfiesCondition(Square toCheck) {
		if (toCheck == null || toCheck.getState().equals(SquareState.UNDISCOVERED))
			return true;
		
		return false;
	}	
};